#include <dalelib/maths.h>
#include <dalelib/alife/robotpart.h>

#include <iostream>
#include <vector>
using namespace std;

namespace dalelib
{

//*******************************************************
RobotPart::RobotPart(dWorldID _World, dSpaceID _Space)
{
	_touch = 0.f;
	_body = dBodyCreate(_World);
	dMassSetZero(&_mass);
}

RobotPart::~RobotPart()
{
	dGeomDestroy(_geom);
	dBodyDestroy(_body);
	glDeleteLists(_display_list_id, 1);
}

void RobotPart::reset()
{
	setPosition(_cached_pos);
	setRotation(_cached_rot);

	dBodySetLinearVel(_body, 0.f, 0.f, 0.f );
	dBodySetAngularVel(_body, 0.f, 0.f, 0.f );
}

void RobotPart::update()
{
	_touch *= 0.8f;//*TIME_STEP;

	const dReal *pos = dBodyGetPosition(_body);
	const dReal *rot = dBodyGetRotation(_body);
	_global_mat = m4(	rot[ 0], rot[ 4], rot[ 8], 0.f,
						rot[ 1], rot[ 5], rot[ 9], 0.f,
						rot[ 2], rot[ 6], rot[10], 0.f,
						pos[ 0], pos[ 1], pos[ 2], 1.f );
}

void RobotPart::draw()
{
//	glColor3f(1.f-_touch,1.f,1.f-_touch);
	glPushMatrix();
	glMultMatrixf(_global_mat.ptr());
	glCallList(_display_list_id);
	glPopMatrix();
}



//*******************************************************
RobotCapsule::RobotCapsule(dWorldID _World, dSpaceID _Space, const float _Radius, const v3 _Position0, const v3 _Position1, const float _Density)
	: RobotPart(_World,_Space)
{
	const float length = (_Position1-_Position0).len();
	_size = v3(_Radius,_Radius,length);
	createCapsule(_size, 16, 16);

	dMassSetCapsule(&_mass, _Density, 3, _size[0], _size[2]);
	dBodySetMass(_body, &_mass);

	_geom = dCreateCCylinder(_Space, _size[0], _size[2]);
	dGeomSetBody(_geom, _body);

	v3 p = (_Position0+_Position1)*0.5f;
	dBodySetPosition(_body, p[0], p[1], p[2]);

	v3 z = (_Position1-_Position0).unit();
	v3 x = v3::cross(z,v3(0,1,0)).unit();
	v3 y = v3::cross(z,x);

	setRotation(m3(x,y,z));

	_cached_pos = getPosition();
	_cached_rot = getRotation();

	update();
}

void RobotCapsule::createCapsule(const v3 _Size, int _Stacks, int _Slices)
{
	_Stacks /= 2; // only need half sphere
	_Slices += 1; // make slices wrap
	const float r = _Size[0];
	const float l = _Size[2]*0.5f;
	int index = 0;
	v3* sphere = new v3[_Stacks*_Slices];

	{ // create unit sphere vertices
		for(int idx=0,j=0; j<_Stacks; ++j)
		{
			const float theta = (float(j)/(_Stacks-1))*(HALFPI);
			const float st = sinf(theta), ct = cosf(theta);
			for(int i=0; i<_Slices; ++i,++idx)
			{
				const float alpha = (float(i)/(_Slices-1))*TWICEPI;
				const float sa = sinf(alpha), ca = cosf(alpha);
				sphere[idx] = v3(ca*st,sa*st,ct);
			}
		}
	}

	_display_list_id = glGenLists(1);
	glNewList(_display_list_id, GL_COMPILE);
	{ // top cap
		glBegin(GL_TRIANGLE_FAN);
		int idx = 0;
		glNormal3f(sphere[idx].x(),sphere[idx].y(),sphere[idx].z());
		glVertex3f(sphere[idx].x()*r,sphere[idx].y()*r,sphere[idx].z()*r+l);
		for(int i=0; i<_Slices; ++i)
		{
			idx = i+_Slices;
			glNormal3f(sphere[idx].x(),sphere[idx].y(),sphere[idx].z());
			glVertex3f(sphere[idx].x()*r,sphere[idx].y()*r,sphere[idx].z()*r+l);
		}
		glEnd();
	}
	{ // top ball
		for(int j=1; j<_Stacks-1; ++j)
		{
			int idx = j*_Slices;
			glBegin(GL_TRIANGLE_STRIP);
			for(int i=0; i<_Slices; ++i,++idx)
			{
				glNormal3f(sphere[idx].x(),sphere[idx].y(),sphere[idx].z());
				glVertex3f(sphere[idx].x()*r,sphere[idx].y()*r,sphere[idx].z()*r+l);
				const int idx2 = idx+_Slices;
				glNormal3f(sphere[idx2].x(),sphere[idx2].y(),sphere[idx2].z());
				glVertex3f(sphere[idx2].x()*r,sphere[idx2].y()*r,sphere[idx2].z()*r+l);
			}
			glEnd();
		}
	}
	{ // tube
		int idx = (_Stacks-1)*_Slices;
		glBegin(GL_TRIANGLE_STRIP);
		for(int i=0; i<_Slices; ++i,++idx)
		{
			glNormal3f(sphere[idx].x(),sphere[idx].y(),sphere[idx].z());
			glVertex3f(sphere[idx].x()*r,sphere[idx].y()*r,l);
			glNormal3f(sphere[idx].x(),sphere[idx].y(),sphere[idx].z());
			glVertex3f(sphere[idx].x()*r,sphere[idx].y()*r,-l);
		}
		glEnd();
	}
	{ // top ball
		for(int j=1; j<_Stacks-1; ++j)
		{
			int idx = j*_Slices;
			glBegin(GL_TRIANGLE_STRIP);
			for(int i=0; i<_Slices; ++i,++idx)
			{
				const int idx2 = idx+_Slices;
				glNormal3f(sphere[idx2].x(),sphere[idx2].y(),-sphere[idx2].z());
				glVertex3f(sphere[idx2].x()*r,sphere[idx2].y()*r,-sphere[idx2].z()*r-l);
				glNormal3f(sphere[idx].x(),sphere[idx].y(),-sphere[idx].z());
				glVertex3f(sphere[idx].x()*r,sphere[idx].y()*r,-sphere[idx].z()*r-l);
			}
			glEnd();
		}
	}
	{ // top cap
		glBegin(GL_TRIANGLE_FAN);
		int idx = 0;
		glNormal3f(sphere[idx].x(),sphere[idx].y(),-sphere[idx].z());
		glVertex3f(sphere[idx].x()*r,sphere[idx].y()*r,-sphere[idx].z()*r-l);
		for(int i=0; i<_Slices; ++i)
		{
			idx = _Slices*2-i-1;
			glNormal3f(sphere[idx].x(),sphere[idx].y(),-sphere[idx].z());
			glVertex3f(sphere[idx].x()*r,sphere[idx].y()*r,-sphere[idx].z()*r-l);
		}
		glEnd();
	}

	glEndList();

	delete [] sphere;
}

//*******************************************************
RobotBox::RobotBox(dWorldID _World, dSpaceID _Space, const float _Radius, const v3 _Position0, const v3 _Position1, const float _Density)
	: RobotPart(_World,_Space)
{
	const float length = (_Position1-_Position0).len()*0.5f;
	_size = v3(_Radius,_Radius,length);
	createBox(_size);

	dMassSetBox(&_mass, _Density, _size[0], _size[1], _size[2]);
	dBodySetMass(_body, &_mass);

	_geom = dCreateBox(_Space, _size[0], _size[1], _size[2]);
	dGeomSetBody(_geom, _body);

	v3 p = (_Position0+_Position1)*0.5f;
	dBodySetPosition(_body, p[0], p[1], p[2]);

	v3 z = (_Position1-_Position0).unit();
	v3 x = v3::cross(z,v3(0,1,0)).unit();
	v3 y = v3::cross(z,x);

	setRotation(m3(x,y,z));

	_cached_pos = getPosition();
	_cached_rot = getRotation();

	update();
}

void RobotBox::createBox(const v3 _Size)
{
	v3 p[] = {
		v3(-_Size[0],-_Size[1],-_Size[2]),
		v3( _Size[0],-_Size[1],-_Size[2]),
		v3( _Size[0], _Size[1],-_Size[2]),
		v3(-_Size[0], _Size[1],-_Size[2]),
		v3(-_Size[0],-_Size[1], _Size[2]),
		v3( _Size[0],-_Size[1], _Size[2]),
		v3( _Size[0], _Size[1], _Size[2]),
		v3(-_Size[0], _Size[1], _Size[2]),
	};

	_display_list_id = glGenLists(1);
	glNewList(_display_list_id, GL_COMPILE);

	glBegin(GL_QUADS);
		glNormal3f( 0, 0,-1); glVertex3fv(p[0].ptr()); glVertex3fv(p[1].ptr()); glVertex3fv(p[2].ptr()); glVertex3fv(p[3].ptr());
		glNormal3f( 1, 0, 0); glVertex3fv(p[1].ptr()); glVertex3fv(p[5].ptr()); glVertex3fv(p[6].ptr()); glVertex3fv(p[2].ptr());
		glNormal3f( 0, 0, 1); glVertex3fv(p[5].ptr()); glVertex3fv(p[4].ptr()); glVertex3fv(p[7].ptr()); glVertex3fv(p[6].ptr());
		glNormal3f(-1, 0, 0); glVertex3fv(p[4].ptr()); glVertex3fv(p[0].ptr()); glVertex3fv(p[3].ptr()); glVertex3fv(p[7].ptr());
		glNormal3f( 0, 1, 0); glVertex3fv(p[3].ptr()); glVertex3fv(p[2].ptr()); glVertex3fv(p[6].ptr()); glVertex3fv(p[7].ptr());
		glNormal3f( 0,-1, 0); glVertex3fv(p[4].ptr()); glVertex3fv(p[5].ptr()); glVertex3fv(p[1].ptr()); glVertex3fv(p[0].ptr());
	glEnd();

	glEndList();
}


} // namespace dalelib
